#ifndef __ROBOT_DISPATCHER_HPP__
#define __ROBOT_DISPATCHER_HPP__

#include "globaldef.hpp"
#include "PlanForLand.hpp"
#include "Map.hpp"
#include <list>
#include <random>
#include "Robot.hpp"
#include "Coord.hpp"
#include <vector>
#include <utility>

/**
 * @brief 机器人调度器
 */
class RobotDispatcher
{
private:
    bool isPlanningUnload = false; // 是否正在规划卸货的路径
    bool crashed = false; // 发生了碰撞
    #ifdef USE_LOG
    int valueSum = 0; // 运输的货物总价值
    int disSum = 0; // 总移动距离
    #endif
    struct cmd_t {
        enum type_t {
            INVALID, // 无效指令
            MOVE, // 移动指令
            LOAD, // 移动并装货指令
            UNLOAD, // 移动并卸货指令
            BLOCK, // 停在原地，变为障碍物
            AVOID, // 无条件避让指令
        } type = INVALID;
        Coord coord = Coord(-1, -1); // 终点坐标
        direction_t dir = direction_t::INVALID; // 移动方向

        cmd_t() = default;
        cmd_t(type_t type, Coord coord = Coord(-1, -1)) :
            type(type), coord(coord), dir(direction_t::INVALID) {}
        cmd_t(type_t type, Coord coord, direction_t dir) :
            type(type), coord(coord), dir(dir) {}
    };

public:
    Map &map; // 地图
    Robot *robot = nullptr; // 机器人
    PlanForLand plan; // 正在执行的方案
    std::list<cmd_t> cmdList; // 指令表
    cmd_t lastCmd; // 上轮指令
    bool isLock = false; // 是否禁用

public:
    /**
     * @brief 构造函数
     * @param map 
     */
    explicit RobotDispatcher(Map &map):
        map(map) {

    }
    #ifdef USE_LOG
    /**
     * @brief 析构函数
     * @return
     */
    ~ RobotDispatcher() {
        if (disSum > 0) {
            LOG("机器人[%d] 价值位移比[%5d]/[%5d]=[%.2f]\n", robot->id, valueSum, disSum, static_cast<float>(valueSum)/disSum);
        }
    }
    #endif
    /**
     * @brief 连接到机器人
     * @param robot 
     */
    void attach(Robot *robot) {
        this->robot = robot;
    }
    /**
     * @brief 锁定该分配器，使其表现为一直处于忙碌状态
     */
    void lock() {
        isLock = true;
    }
    /**
     * @brief 是否空闲
     * @return true 
     * @return false 
     */
    bool isFree() const {
        return !isLock && !plan.isValid();
    }
    /**
     * @brief 清空指令表产生的时空障碍物
     */
    void clearCmdTimeBlock() {
        int time = 1;
        for (auto const &cmd : cmdList) { // clear时要移除时空障碍物
            if (cmd.type != cmd_t::INVALID && cmd.type != cmd_t::BLOCK) {
                map.deleteTimeBlock(time, cmd.coord, cmd.dir);
            }
            ++time;
        }
    }
    /**
     * @brief 清空指令表
     */
    void clearCmdList() {
        clearCmdTimeBlock(); // 先清空时空障碍物
        cmdList.clear();
    }
    /**
     * @brief 工作循环
     * @param frameID
     */
    void run(int frameID) {
        // 方案有效性检查
        if (lastCmd.type == cmd_t::LOAD && !robot->carried && robot->running) { // 取货失败，方案无效
            LOG("[%05d] 机器人[%d]取货失败，货物价值[%03d] --\n", frameID, robot->id, plan.goods->value);
            plan.setValidate(false);
            plan.goods = nullptr;
            clearCmdList();
        }

        // 死锁检查
        if (robot->running) {
            for (auto const &cmd : cmdList) {
                if (cmd.type != cmd_t::INVALID && cmd.type != cmd_t::BLOCK) {
                    if (map.isBlock(cmd.coord)) { // 路线中出现了死锁点，说明机器人的规划限制了其余机器人的移动
                        clearCmdList();
                        break;
                    }
                }
            }
        }

        // 规划路线
        if (plan.isValid() && robot->running) {
            while (cmdList.size() < PARAMS.PLAN_STEPS) { // 规划到指定步数
                if (!cmdList.empty() && cmdList.back().type == cmd_t::UNLOAD) { // 如果指令最后一个序列是卸货，说明已规划完毕
                    break;
                }
                Coord &coord = cmdList.empty() ? *(this->robot) : cmdList.back().coord;
                int minDistance = DISTANCE_INF;
                cmd_t nextCmd(cmd_t::MOVE, coord);
                int time = cmdList.size() + 1; // 新指令执行的时间
                direction_t timeBlockDir = direction_t::INVALID; // 用于存放可能会发生碰撞的时空障碍物的方向
                std::vector<std::pair<Coord, direction_t>> canUse; // 可用的坐标和方向
                if (!isPlanningUnload) { // 规划的是装货路径
                    for (int i = 0; i <= 4; ++i) { // 包含原地不动
                        direction_t dir = static_cast<direction_t>(i);
                        Coord newCoord(coord, dir);
                        if (newCoord.isInRect(0, 0, MAP_X_MAX, MAP_Y_MAX) && map.isLandPath(newCoord) && !map.willCrashTimeBlock(time, newCoord, dir, &timeBlockDir)) {
                            canUse.push_back(std::make_pair(newCoord, dir));
                        }
                    }
                    for (auto const &it : canUse) {
                        int newDistance = (*plan.goods->distance)[it.first.x][it.first.y];
                        if (newDistance < minDistance) {
                            minDistance = newDistance;
                            nextCmd.coord = it.first;
                            nextCmd.dir = it.second;
                        } else if (newDistance == minDistance && timeBlockDir != direction_t::INVALID && it.second != timeBlockDir) { // 尽量不沿着时空障碍物的方向回退
                            minDistance = newDistance;
                            nextCmd.coord = it.first;
                            nextCmd.dir = it.second;
                        }
                    }
                    if (minDistance == 0) {
                        nextCmd.type = cmd_t::LOAD;
                        isPlanningUnload = true;
                    }
                } else { // 规划的是卸货路径
                    for (int i = 0; i <= 4; ++i) { // 包含原地不动
                        direction_t dir = static_cast<direction_t>(i);
                        Coord newCoord(coord, dir);
                        if (newCoord.isInRect(0, 0, MAP_X_MAX, MAP_Y_MAX) && (map.isLandPath(newCoord) || map.isBerth(newCoord)) && !map.willCrashTimeBlock(time, newCoord, dir, &timeBlockDir)) {
                            canUse.push_back(std::make_pair(newCoord, dir));
                        }
                    }
                    for (auto const &it : canUse) {
                        int newDistance = map.getBerthDistance(plan.berthID, it.first);
                        if (newDistance < minDistance) {
                            minDistance = newDistance;
                            nextCmd.coord = it.first;
                            nextCmd.dir = it.second;
                        } else if (newDistance == minDistance && timeBlockDir != direction_t::INVALID && it.second != timeBlockDir) { // 尽量不沿着时空障碍物的方向回退
                            minDistance = newDistance;
                            nextCmd.coord = it.first;
                            nextCmd.dir = it.second;
                        }
                    }
                    if (minDistance == 0) {
                        nextCmd.type = cmd_t::UNLOAD;
                    }
                }
                if (minDistance == DISTANCE_INF) { // 无处可去
                    clearCmdList(); // 清空队列，在下个环节设为障碍物
                    break;
                } else {
                    cmdList.push_back(nextCmd);
                    map.addTimeBlock(nextCmd.coord, nextCmd.dir, time);
                }
            }
        }

        // 指令队列为空的机器人无条件避让
        if (cmdList.empty() && robot->running) {
            cmdList.push_back(cmd_t(cmd_t::AVOID));
        }

        // 取指运行
        if (robot->running) { // 机器人正常运行
            if (crashed) { // 碰撞恢复的首帧处理
                crashed = false; // 恢复碰撞标志位
                map.deleteBlock(*robot); // 解除永久障碍物
                int time = 1;
                for (auto const &cmd : cmdList) { // 重新添加指令表中指令产生的的时空障碍物
                    if (cmd.type != cmd_t::INVALID && cmd.type != cmd_t::BLOCK) {
                        map.addTimeBlock(cmd.coord, cmd.dir, time);
                    }
                    ++time;
                }
            }
            if (lastCmd.type == cmd_t::BLOCK) { // 解锁地图块
                map.deleteBlock(*robot);
            }
            lastCmd.type = cmd_t::INVALID; // 清除上一轮指令记录
            if (!cmdList.empty()) {
                auto cmd = cmdList.front();
                cmdList.pop_front();
                if (cmd.type != cmd_t::AVOID) {
                    lastCmd = cmd;
                }
                switch (cmd.type) {
                case cmd_t::AVOID: {
                    direction_t timeBlockDir = direction_t::INVALID; // 用于存放可能会发生碰撞的时空障碍物的方向
                    if (map.isBerth(*robot) || map.getBerthDistance(map.getNearestBerth(*robot), *robot) <= PARAMS.FREE_AWAY_DISTANCE || map.willCrashTimeBlock(1, *robot, direction_t::INPLACE, &timeBlockDir)) { // 如果在泊位内或停在原地会碰到时空障碍物
                        std::vector<std::pair<Coord, direction_t>> canUse; // 可用的坐标和方向
                        for (int i = 1; i <= 4; ++i) { // 寻找可能的避障方向
                            direction_t dir = static_cast<direction_t>(i);
                            Coord newCoord(*robot, dir);
                            if (newCoord.isInRect(0, 0, MAP_X_MAX, MAP_Y_MAX) && map.isLandPath(newCoord, false) && !map.willCrashTimeBlock(1, newCoord, dir)) { // 不允许向泊位内避障
                                canUse.push_back(std::make_pair(newCoord, dir));
                            }
                        }
                        std::pair<Coord, direction_t> best = std::make_pair(Coord(-1, -1), direction_t::INVALID);
                        for (auto const &it : canUse) {
                            best = it;
                            if (timeBlockDir != direction_t::INVALID && it.second != timeBlockDir) { // 尽量沿着与时空障碍物不同的方向走，避免一直被推着走
                                break;
                            }
                        }
                        if (best.second != direction_t::INVALID) {
                            robot->move(best.second);
                            #ifdef USE_LOG
                            ++disSum;
                            #endif
                            map.addTimeBlock(best.first, best.second);
                        } else { // 没找到躲避方法，设为永久障碍物，本次指令视为执行了cmd_t::BLOCK
                            map.addBlock(*robot);
                            lastCmd = cmd_t::BLOCK;
                        }
                    } else {
                        map.addTimeBlock(*robot);
                    }
                }
                    break;
                case cmd_t::BLOCK:
                    map.addBlock(*robot);
                    break;
                case cmd_t::MOVE:
                    robot->move(cmd.dir);
                    #ifdef USE_LOG
                    ++disSum;
                    #endif
                    break;
                case cmd_t::LOAD:
                    robot->move(cmd.dir);
                    #ifdef USE_LOG
                    ++disSum;
                    #endif
                    robot->load();
                    break;
                case cmd_t::UNLOAD:
                    robot->move(cmd.dir);
                    robot->unload();
                    #ifdef USE_LOG
                    ++disSum;
                    valueSum += plan.goods->value;
                    #endif
                    plan.setValidate(false);
                    break;
                default:
                    break;
                }
            }
        } else { // 机器人受到碰撞
            if (!crashed) { // 碰撞的首帧处理
                #ifdef USE_LOG
                LOG("[%05d] 机器人[%d]发生碰撞，最后指令[%d]-----\n", frameID, robot->id, lastCmd.type);
                #endif
                crashed = true; // 设置碰撞标志位
                map.addBlock(*robot); // 将当前位置设置为永久障碍物
                clearCmdTimeBlock(); // 重置时空障碍物
                cmdList.push_front(lastCmd); // 将上一帧指令压回队头
            }
        }
    }
    /**
     * @brief 设置方案
     * @param plan 
     */
    void setPlan(const PlanForLand &plan) {
        this->plan = plan;
        if (!plan.isValid()) {
            return;
        }
        plan.goods->setCarried(); // 从设置方案起就算作货物已被携带
        isPlanningUnload = false; // 变为装货规划状态
    }
};

#endif // __ROBOT_DISPATCHER_HPP__